/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "CTopoScanOccuMCL.h"

CTopoScanOccuMCL::CTopoScanOccuMCL(){
		O=NULL;
		dist = NULL;
		prob_var = 0;
		prob_max = 0;
		measurement_variance = 2.0;
		vmeas = measurement_variance*measurement_variance;
}
CTopoScanOccuMCL::~CTopoScanOccuMCL(){
	
	if(dist) free(dist);
	if(O) delete(O);
}

/**
 * Initializes the filter - uses normal random method for calculating the initial distribution
 * Loads the map from file, reserves memory for particles, sets the initial distribution
 * @p *mapfile The mapfile name
 * @p mapscale the scale factor needed to get the map coordinates into meters
 * @p num_of_particles The number of particles in the filter
 * @p init_pose the initial pose
 * @p initVariance the variance for the initial quess
 * @p num_of_meas_in_scan number of measurements used from the scan
 * @p resolution The resolution of the occupancy grid (in meters)
 * @p lineval The value the line map is set to in grid
 */
void CTopoScanOccuMCL::initialize(const char *mapfile,
								float mapscale, 
								int num_of_particles, 
								mcl::pose initPose, 
								mcl::pose initVariance,
								int num_of_meas_in_scan,
								float _sensor_offset,
								float resolution,
								grid_t lineval)
{
		///< initializing with small value, will be resized when linemap is read
		O = new TOccupancyGrid(1,1,resolution,Grid::TCoord2D(0,0));
		fprintf(stderr,"scale=%f reso=%f lineval=%f\n",mapscale,resolution,lineval); 
		if(!O->loadLineMap(mapfile, mapscale,lineval)){
				fprintf(stderr,"CTopoScanOccuMCL::initializeVariables::Unable to load the map\n");
				exit(1);
		}								
		
		
		initializeVariables(lineval,num_of_meas_in_scan,_sensor_offset);
				
		filt.initializeNormalRandom(initPose, initVariance,num_of_particles);													
}
/**
 * Initializes the filter - uses normal random method for calculating the initial distribution
 * Loads the map from line file, reserves memory for particles, sets the initial distribution
 * @p *mapfile The mapfile name
 * @p num_of_particles The number of particles in the filter
 * @p init_pose the initial pose
 * @p initVariance the variance for the initial quess
 * @p num_of_meas_in_scan number of measurements used from the scan
*/

void CTopoScanOccuMCL::initialize(const char *mapfile,
								int num_of_particles, 
								mcl::pose initPose, 
								mcl::pose initVariance,
								int num_of_meas_in_scan,
								float _sensor_offset,
								grid_t occu_val)
{
	///< initializing with small value, will be resized when linemap is read
	O = new TOccupancyGrid(1,1,0.1,Grid::TCoord2D(0,0));
	 
	if(!O->load(mapfile)){
		fprintf(stderr,"CTopoScanOccuMCL::initializeVariables::Unable to load the map\n");
		exit(1);
	}
	initializeVariables(occu_val,num_of_meas_in_scan,_sensor_offset);
				
	filt.initializeNormalRandom(initPose, initVariance,num_of_particles);													
}


/**
 * Initializes the filter - uses uniform distribution method for calculating the initial distribution
 * Loads the map from file, reserves memory for particles, sets the initial distribution
 * The initial distribution is calculated using bounding box of the map. The @dP sets the 
 * resolution of the unform grid.  WARNING the initialization might not be 
 * real-time.
 * @p *mapfile The mapfile name
 * @p mapscale the scale factor needed to get the map coordinates into meters
 * @p dP the resolution for the uniform grid
 * @p num_of_meas_in_scan number of measurements used from the scan
 */
void CTopoScanOccuMCL::initializeGlobal(const char *mapfile,
																		float mapscale,
															float resolution,  
							 								mcl::pose dP, 
															mcl::pose start,
															mcl::pose end,
															int num_of_meas_in_scan,
															float _sensor_offset)
{
		initializeVariables((grid_t) 1.0,num_of_meas_in_scan,_sensor_offset);
		filt.initializeUniform(start, end,dP);
}							

/**
 * Initialises the variables
*/
void CTopoScanOccuMCL::initializeVariables(//const char *mapfile,
															//float mapscale,  
															//float resolution,
															grid_t lineval,
															int num_of_meas_in_scan,
															float _sensor_offset)
{
		number_of_meas = num_of_meas_in_scan;
		sensor_offset=_sensor_offset;
		topo_max_dist = 99.0;
		topo_min_dist = 0.4;
		
		virt.alloc(num_of_meas_in_scan);
		
	
		o_val = lineval;
		int Nx,Ny;
		O->getDim(Nx, Ny);
		dist = (float *) malloc(Nx*Ny*sizeof(float));
		fprintf(stderr,"CTopoScanOccuMCL::initializeVariables(): Computing distance transform\n");
		O->distanceTransform(dist, lineval);	 
		float *dataPtr=O->getDataPtr();
	///Now lets make us a Likelihood function
		//for(int i=0;i<Nx*Ny;i++){
		//	if(dist[i]>5) dist[i]=0;
		//	if(dist[i]>0.3) dist[i]=0.3; ///Everything further than 0.3m away from obstacles get the same value
		//	dataPtr[i] = dist[i]; 			///< Copy to occupancy grid
		//}
	///Thats it :) Now we have a precomputed 2D likelihood function
		
		fprintf(stderr,"CTopoScanOccuMCL::Variables initialized\n");
}
/**
 * Same as init but without mallocs
 */
void CTopoScanOccuMCL::reinitializeNR(mcl::pose initPose,mcl::pose initVariance){
		filt.initializeNormalRandom(initPose, initVariance,filt.NumOfParticles);	
}

/**
 * Run the MCL step
 * @p dP the relative movement between steps
 * @p variance prediction error 
 * @p measurement the scan taken from new position
 */
void CTopoScanOccuMCL::runMCL(mcl::pose dP,mcl::pose variance, mcl::scan measurement){
		mcl::scan mcl_meas;
		mcl::pose var;
		static float sinceSIR=0; 
		//float sensor_offset_tmp;
		
		filt.predict(dP,variance); ///<Predict
		
		if(sqrt(last_meas_update.x*last_meas_update.x + last_meas_update.y*last_meas_update.y)>.2 || 
				 fabs(last_meas_update.a)>(15.0*M_PI/180.0))
		{
				last_meas_update.set(0,0,0);
				fprintf(stderr,"Updating the measurement... !!");
				int fac = measurement.N / number_of_meas; 		
				mcl_meas.allocate(number_of_meas);
				mcl_meas.N=number_of_meas;
				///Take the measurements that will be used in estimation
				/// The number is reduced for computational & 
				///efficiency reasons i.e. MCL must not be "too accurate"
				for(int i=0;i<number_of_meas;i++){
						mcl_meas.r[i] = measurement.r[fac*i];	
						mcl_meas.a[i] = measurement.a[fac*i];
						if(fac*i>=measurement.N) 
								fprintf(stderr,"runMCL:: Terrible mistake - expect crash!\n");
				}
				
				calculateLikelihood(mcl_meas); ///<Calculate likelihoods
			//	fprintf(stderr,".... Done\n");
				
				
		}else{
				last_meas_update = last_meas_update.integrateDifferential(dP);
				calculateLikelihood();
		}
		
		filt.normalize(); 
		var = filt.getDistributionVariances(); 
		//float rad = sqrt(var.x*var.x + var.y*var.y + var.a*var.a);
		
		///Calculate the variance of the probabilities
		float varP=0;
		float iN = 1.0 / (float) filt.NumOfParticles;
		float maxP = -1;
		for(int i = 0; i<filt.NumOfParticles;i++){
				varP += (filt.Particles[i].p - iN)*(filt.Particles[i].p - iN);
				if(filt.Particles[i].p > maxP) maxP = filt.Particles[i].p;
		}
		prob_max = maxP;
		prob_var = varP;
		///Using now stetson harrison method
		//fprintf(stderr,"%f \n",varP);
		if((varP > .001 || sinceSIR > 250)&&sinceSIR>2){
				fprintf(stderr,"Performing SIR varP=%.3f\n",varP);
				filt.SIRUpdate();
				sinceSIR = 0;
				prob_var = 0;
				prob_max = 1/filt.NumOfParticles;
		}
		sinceSIR++;
		
		free(mcl_meas.r);
		free(mcl_meas.a);
}

/**
 * Run the MCL step in initialization mode
 * @p dP the relative movement between steps
 * @p variance prediction error 
 * @p measurement the scan taken from new position
 */
void CTopoScanOccuMCL::runMCLInit(mcl::pose dP,mcl::pose variance, mcl::scan measurement){
		mcl::scan mcl_meas;
		mcl::pose var;
		static int sinceSIR=2;
		
		///< The ratio between the measurement and wanted num_of_meas
		int fac = measurement.N / number_of_meas; 
		
		mcl_meas.allocate(number_of_meas);
		mcl_meas.N=number_of_meas;
		///Take the measurements that will be used in estimation
		/// The number is reduced for computational & efficiency reasons i.e. MCL must not be "too accurate"
		for(int i=0;i<number_of_meas;i++){
			//fprintf(stderr,"i=%d faci=%d ",i,fac*i);
				mcl_meas.r[i] = measurement.r[fac*i];	
				mcl_meas.a[i] = measurement.a[fac*i];
				if(fac*i>=measurement.N) 
						fprintf(stderr,"runMCL:: Terrible mistake - expect crash!\n");
		}
		
		filt.predict(dP,variance); ///<Predict
		calculateLikelihood(mcl_meas); ///<Calculate likelihoods
		filt.normalize(); 
		var = filt.getDistributionVariances(); 

		filt.SIRUpdate(); ///< allways update!
		sinceSIR++;
		
		free(mcl_meas.r);
		free(mcl_meas.a);
}


/**
 * Calculates the likelihood of particle distribution
 * uses the CParticleFilter filt and TLineMap map
 * @param meas The current measurement
 * Uses the private members:
				float *virtualr;
				float *virtuallines;
				float *virtualscan_xy;
				float *virtualscan_xy_rel;
				int num_of_meas;
 **/
void CTopoScanOccuMCL::calculateLikelihood(mcl::scan meas)
{
		int i;		
		int n=0;
		filt.Lik = 0;
		filt.outLiers = 0;
		float Lik,Liks;
		///Cycle through each particle
		//fprintf(stderr,"Numofmeas=%d, num particles=%d\n",num_of_meas,filt.NumOfParticles);
		grid_t v;
		
		float dl,dL2;
		//float maxlik = exp(  - (0.01/(vmeas)) );
		Grid::TPixel pix;
		int Nx,Ny;
		O->getDim(Nx,Ny);
		
		for(i=0;i<filt.NumOfParticles;i++){
				pix = O->toPixel(Grid::TCoord2D(filt.Particles[i].x, filt.Particles[i].y));
				v = dist[pix.y*Nx+pix.x];
				if(v>topo_max_dist) v=0;
				else if(v>topo_min_dist) v=topo_min_dist;
				
				
// 				O->getScan(filt.Particles[i].x,
// 									 filt.Particles[i].y, 
// 									 filt.Particles[i].a+sensor_offset, 
// 									 number_of_meas, 
// 									 M_PI/number_of_meas, 
// 									 o_val,
// 									 dist, 
// 									 virt);
				Liks = v;
				n=0;
				for(int j=0;j<number_of_meas;j++){
					virt.r[j] = O->getLine(filt.Particles[i].x,
																 filt.Particles[i].y, 
																 filt.Particles[i].a+sensor_offset+meas.a[j], 
																 o_val, dist);
						
						dl = meas.r[j] - virt.r[j];		// The difference between expected measurement and real measurement
						
						//If the value was ok
						//if(meas.r[j]>0 && virt.r[j]>0 && dl>-2.0){
						#warning "WARNING:HARD CODED STUFF JUST APPEARED IN TO THE DRIVER!!!!!"
							if(meas.r[j]>0 && virt.r[j]>0 && meas.r[j]<4.0 && virt.r[j]<4.0){
								//if(fabs(dl)<0.1) dl=0.1;
								//if(dl>2.0) dl=2.0;
								dL2 = dl*dl;
								Lik = 0.5 + 0.5*exp(  - (dL2/(vmeas)) );
								n++;
								Liks *= Lik;
								//fprintf(stderr,"|");
						}
						
				}
				//fprintf(stderr,"\n");
				//if(Liks<0.01) Liks=0.01;
				/// Average of the probabilities...
				/*
				if(n>0){
						Liks /= n;
				}else{
						Liks = 0.5;
				}
				*/
				Liks =0.99*Liks+0.01; ///< Scale the probability to 0.5...1
				
				
				filt.Particles[i].lik=(Liks);
				
				filt.Lik+=Liks;
		}
}
/**
* Update likelihood without the laser measurement
**/
void CTopoScanOccuMCL::calculateLikelihood()
{
		int i;		
		filt.Lik = 0;
		filt.outLiers = 0;
		///Cycle through each particle
		//fprintf(stderr,"Numofmeas=%d, num particles=%d\n",num_of_meas,filt.NumOfParticles);
		grid_t v;
		Grid::TPixel pix;
		int Nx,Ny;
		O->getDim(Nx,Ny);
		for(i=0;i<filt.NumOfParticles;i++){	
				pix = O->toPixel(Grid::TCoord2D(filt.Particles[i].x, filt.Particles[i].y));
				v = dist[pix.y*Nx+pix.x];
				if(v>topo_max_dist) v=0;
				else if(v>topo_min_dist) v=topo_min_dist;
				filt.Lik+=v;
				filt.Particles[i].lik=v;
		}
}


void CTopoScanOccuMCL::getScan(mcl::scan &meas, mcl::pose &p){
	Grid::TScan out;
	out.alloc(meas.N);
	
	O->getScan(p.x,
						 p.y, 
						 p.a+sensor_offset, 
						 meas.N, 
						 M_PI/meas.N, 
						 o_val,
						 dist, 
						 out);
			
	float a=sensor_offset;
	float da=(M_PI)/meas.N;
	
	for(int i = 0;i<meas.N;i++){
		meas.r[i] = out.r[i];
		meas.a[i] = a;
		a+=da;
	}
	
  out.freeData();
}
void CTopoScanOccuMCL::updateDt(){
	O->distanceTransform(dist, o_val);	
}